/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// schunk_low
#include <schunk_low/schunk.h>
#include <schunk_low/schunk_low_control.h>

// General
#include <string>
#include <vector>

// Messages
#include <schunk_msgs/AllParameters.h>
#include <schunk_msgs/JointState.h>
#include <schunk_msgs/JointStateComplete.h>
#include <schunk_msgs/JointStateElectrical.h>

/******************************************************************************
                                  MAIN ROUTINE
 *****************************************************************************/

int main(int argc, char** argv)
{
    // Initialize ROS, specify name of node
    ros::init(argc, argv, "schunk_low_control");

    // Create SchunkLowControl object
    SchunkLowControl schunk_control;
    schunk_control.GetROSParameters();
    schunk_control.GetRobotDescriptionParameters();
    schunk_control.GetPIDParameters();

    // Main loop
    ros::Rate loop_rate(schunk_control.frequency_);  // Hz

    std::vector<double> lim_vel;
    if (schunk_control.has_gripper_)
        lim_vel.resize(schunk_control.schunk_->module_count() - 1);
    else
        lim_vel.resize(schunk_control.schunk_->module_count());

    while (schunk_control.nodehandle_.ok())
    {
        // Check for called services
        schunk_control.service_server_queue_.callAvailable((ros::WallDuration()));

        if (schunk_control.initialized_)
        {
            // if velocity control mode is selected, check for velocity messages
            if (schunk_control.control_mode_ == CONTROL_MODE_VELOCITY)
            {
                if (!schunk_control.velocity_queue_.isEmpty())
                {
                    schunk_control.velocity_queue_.callAvailable((ros::WallDuration()));
                }
                else
                {
                    if (schunk_control.CheckJointLimits(schunk_control.schunk_->GetCurrentPositions(), schunk_control.schunk_->GetCurrentVelocities(), &lim_vel))
                    {
                        schunk_control.schunk_->SetVelocitiesForNextPosture(lim_vel);
                        schunk_control.schunk_->Move();
                    }
                }
            }

            // if position control mode is selected, check for position messages
            else if (schunk_control.control_mode_ == CONTROL_MODE_POSITION)
            {
                schunk_control.position_queue_.callAvailable((ros::WallDuration()));
            }

            // publish status and debug information
            if ((ros::Time::now() - schunk_control.last_publish_time_) >= schunk_control.min_publish_duration_)
            {
                schunk_control.PublishState();
            }
        }

        loop_rate.sleep();
    }

    return 0;
}

/******************************************************************************
                                    CONSTRUCTORS AND DESTRUCTORS
 *****************************************************************************/

SchunkLowControl::SchunkLowControl()
{
    schunk_ = new Schunk(schunk_nodehandle_);

    // Assignment of the defined callback queues to each node handler
    service_server_nodehandle_.setCallbackQueue(&service_server_queue_);
    publisher_nodehandle_.setCallbackQueue(&publisher_queue_);
    velocity_nodehandle_.setCallbackQueue(&velocity_queue_);
    position_nodehandle_.setCallbackQueue(&position_queue_);

    // Implementation of topics to publish
    publisher_parameters_ = publisher_nodehandle_.advertise<schunk_msgs::AllParameters>("/schunk_low/status/parameters", 1);
    publisher_simple_ = publisher_nodehandle_.advertise<schunk_msgs::JointState>("/schunk_low/status/joint_states_simple", 1);
    publisher_complete_ = publisher_nodehandle_.advertise<schunk_msgs::JointStateComplete>("/schunk_low/status/joints_states_complete", 1);
    publisher_electrical_ = publisher_nodehandle_.advertise<schunk_msgs::JointStateElectrical>("/schunk_low/status/joints_states_electrical", 1);

    // Implementation of topics to subscribe
    subscriber_velocity_ = velocity_nodehandle_.subscribe("/schunk_low/control/velocities", 1, &SchunkLowControl::VelocityCallback, this);
    subscriber_position_ = position_nodehandle_.subscribe("/schunk_low/control/positions", 1, &SchunkLowControl::PositionCallback, this);

    // Implementation of service servers
    service_server_init_ = service_server_nodehandle_.advertiseService("/schunk_low/init", &SchunkLowControl::InitCallback, this);
    service_server_home_ = service_server_nodehandle_.advertiseService("/schunk_low/home", &SchunkLowControl::HomeCallback, this);
    service_server_stop_ = service_server_nodehandle_.advertiseService("/schunk_low/stop", &SchunkLowControl::StopCallback, this);
    service_server_recover_ = service_server_nodehandle_.advertiseService("/schunk_low/recover", &SchunkLowControl::RecoverCallback, this);

    service_server_gripper_open_ = service_server_nodehandle_.advertiseService("/schunk_low/gripper/open", &SchunkLowControl::GripperOpenCallback, this);
    service_server_gripper_close_ = service_server_nodehandle_.advertiseService("/schunk_low/gripper/close", &SchunkLowControl::GripperCloseCallback, this);
    service_server_gripper_set_vel_ = service_server_nodehandle_.advertiseService("/schunk_low/gripper/set_velocity", &SchunkLowControl::GripperSetVelCallback, this);

    service_server_connect_ = service_server_nodehandle_.advertiseService("/schunk_low/connect", &SchunkLowControl::ConnectCallback, this);
    service_server_disconnect_ = service_server_nodehandle_.advertiseService("/schunk_low/disconnect", &SchunkLowControl::DisconnectCallback, this);
    service_server_control_mode_ = service_server_nodehandle_.advertiseService("/schunk_low/control_mode", &SchunkLowControl::SetControlModeCallback, this);
    service_server_get_pos_vel_ = service_server_nodehandle_.advertiseService("/schunk_low/get_pos_vel", &SchunkLowControl::GetPosVelCallback, this);

    // Setting status variables
    initialized_ = false;
    connected_ = false;
    last_publish_time_ = ros::Time::now();
    control_mode_ = CONTROL_MODE_VELOCITY;
}

SchunkLowControl::~SchunkLowControl()
{
    schunk_->Close();
    delete schunk_;
}

/******************************************************************************
                                     PARAMETER LOADING FUNCTIONS
 *****************************************************************************/

bool SchunkLowControl::GetROSParameters()
{
    bool error = false;

    // Can Module
    std::string can_module;
    if (nodehandle_.hasParam("/schunk_low_control/can_module"))
    {
        nodehandle_.getParam("/schunk_low_control/can_module", can_module);
    }
    else
    {
        ROS_ERROR("Parameter can_module not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Can Device
    std::string can_device;
    if (nodehandle_.hasParam("/schunk_low_control/can_device"))
    {
        nodehandle_.getParam("/schunk_low_control/can_device", can_device);
    }
    else
    {
        ROS_ERROR("Parameter can_device not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Can Baudrate
    int can_baudrate;
    if (nodehandle_.hasParam("/schunk_low_control/can_baudrate"))
    {
        nodehandle_.getParam("/schunk_low_control/can_baudrate", can_baudrate);
    }
    else
    {
        ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Control Frequency
    int control_frequency;
    if (nodehandle_.hasParam("/schunk_low_control/control_frequency"))
    {
        nodehandle_.getParam("/schunk_low_control/control_frequency", control_frequency);
    }
    else
    {
        ROS_WARN_STREAM("Parameter control_frequency not set, using standard frequency (10Hz)");
        control_frequency = 10;
    }

    // Control Frequency
    double publisher_frequency;
    if (nodehandle_.hasParam("/schunk_low_control/publish_frequency"))
    {
        nodehandle_.getParam("/schunk_low_control/publish_frequency", publisher_frequency);
    }
    else
    {
        ROS_WARN_STREAM("Parameter publish_frequency not set, using standard frequency (1Hz)");
        publisher_frequency = 1.0;
    }

    if (error){
        return false;
    }

    schunk_->SetCanParameters(can_module, can_device, can_baudrate);
    frequency_ = control_frequency;
    min_publish_duration_.fromSec(1.0/publisher_frequency);

    return true;
}

bool SchunkLowControl::GetRobotDescriptionParameters()
{
    bool error = false;

    // Has gripper
    std::string has_gripper_str;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/has_gripper"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/has_gripper", has_gripper_str);
    }
    else
    {
        ROS_ERROR("Parameter has_gripper not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }
    if (has_gripper_str.compare("YES"))
    {
        has_gripper_ = true;
    }
    else
    {
        has_gripper_ = false;
    }

    // Module Count
    int module_count;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/module_count"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/module_count", module_count);
    }
    else
    {
        ROS_ERROR("Parameter module_count not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Position Tolerance
    double position_tolerance;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/position_tolerance"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/position_tolerance", position_tolerance);
    }
    else
    {
        ROS_ERROR("Parameter position_tolerance not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Module IDs
    XmlRpc::XmlRpcValue module_ids_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/module_ids"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/module_ids", module_ids_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter module_ids not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Module Names
    XmlRpc::XmlRpcValue module_names_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/module_names"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/module_names", module_names_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter joint_names not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Max Positions
    XmlRpc::XmlRpcValue max_positions_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/max_positions"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/max_positions", max_positions_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter upper_limits not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Min Positions
    XmlRpc::XmlRpcValue min_positions_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/min_positions"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/min_positions", min_positions_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter lower_limits not set, shutting down node...");

        nodehandle_.shutdown();
        error = true;
    }

    // Home Offsets
    XmlRpc::XmlRpcValue home_offsets_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/home_offsets"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/home_offsets", home_offsets_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter offsets not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Home Positions
    XmlRpc::XmlRpcValue home_positions_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/home_positions"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/home_positions", home_positions_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter offsets not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Home Velocities
    XmlRpc::XmlRpcValue home_velocities_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/home_velocities"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/home_velocities", home_velocities_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter home_velocities not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Max Velocities
    XmlRpc::XmlRpcValue max_velocities_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/max_velocities"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/max_velocities", max_velocities_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter max_velocities not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Max Accelerations
    XmlRpc::XmlRpcValue max_accelerations_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/max_accelerations"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/max_accelerations", max_accelerations_xml_rpc);
    }


    else
    {
        ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Standard Velocities (for position control only)
    XmlRpc::XmlRpcValue std_velocities_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/std_velocities"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/std_velocities", std_velocities_xml_rpc);
    }
    else
    {
        ROS_ERROR("Parameter std_velocities not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    // Standard Accelerations (for position control only
    XmlRpc::XmlRpcValue std_accelerations_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/std_accelerations"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/std_accelerations", std_accelerations_xml_rpc);
    }

    else
    {
        ROS_ERROR("Parameter std_accelerations not set, shutting down node...");
        nodehandle_.shutdown();
        error = true;
    }

    if (error){
        return false;
    }

    std::vector<int> module_ids;
    std::vector<std::string> module_names;
    std::vector<double> max_positions;
    std::vector<double> min_positions;
    std::vector<double> home_offsets;
    std::vector<double> home_positions;
    std::vector<double> home_velocities;
    std::vector<double> max_velocities;
    std::vector<double> max_accelerations;

    module_ids.resize(module_ids_xml_rpc.size());
    module_names.resize(module_names_xml_rpc.size());
    max_positions.resize(max_positions_xml_rpc.size());
    min_positions.resize(min_positions_xml_rpc.size());
    home_offsets.resize(home_offsets_xml_rpc.size());
    home_positions.resize(home_positions_xml_rpc.size());
    home_velocities.resize(home_velocities_xml_rpc.size());
    max_velocities.resize(max_velocities_xml_rpc.size());
    max_accelerations.resize(max_accelerations_xml_rpc.size());

    for (int i = 0; i < module_ids_xml_rpc.size(); i++) module_ids[i] = static_cast<int>(module_ids_xml_rpc[i]);
    for (int i = 0; i < module_names_xml_rpc.size(); i++) module_names[i] = static_cast<std::string>(module_names_xml_rpc[i]);
    for (int i = 0; i < max_positions_xml_rpc.size(); i++) max_positions[i] = static_cast<double>(max_positions_xml_rpc[i]);
    for (int i = 0; i < min_positions_xml_rpc.size(); i++) min_positions[i] = static_cast<double>(min_positions_xml_rpc[i]);
    for (int i = 0; i < home_offsets_xml_rpc.size(); i++) home_offsets[i] = static_cast<double>(home_offsets_xml_rpc[i]);
    for (int i = 0; i < home_positions_xml_rpc.size(); i++) home_positions[i] = static_cast<double>(home_positions_xml_rpc[i]);
    for (int i = 0; i < home_velocities_xml_rpc.size(); i++) home_velocities[i] = static_cast<double>(home_velocities_xml_rpc[i]);
    for (int i = 0; i < max_velocities_xml_rpc.size(); i++) max_velocities[i] = static_cast<double>(max_velocities_xml_rpc[i]);
    for (int i = 0; i < max_accelerations_xml_rpc.size(); i++) max_accelerations[i] = static_cast<double>(max_accelerations_xml_rpc[i]);

    schunk_->SetRobotParameters(module_count, module_ids, module_names, max_positions, min_positions, home_offsets, home_positions, home_velocities, max_velocities, max_accelerations);

    max_pos_.resize(module_count);
    min_pos_.resize(module_count);
    for (int i = 0; i < module_count; i++)
    {
        max_pos_[i] = max_positions[i] * position_tolerance;
        min_pos_[i] = min_positions[i] * position_tolerance;
    }

    std_vel_.resize(std_velocities_xml_rpc.size());
    std_acc_.resize(std_accelerations_xml_rpc.size());
    for (int i = 0; i < std_velocities_xml_rpc.size(); i++) std_vel_[i] = static_cast<double>(std_velocities_xml_rpc[i]);
    for (int i = 0; i < std_accelerations_xml_rpc.size(); i++) std_acc_[i] = static_cast<double>(std_accelerations_xml_rpc[i]);

    return true;
}

bool SchunkLowControl::GetPIDParameters()
{
    // Parameter A0
    XmlRpc::XmlRpcValue A0_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/A0"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/A0", A0_xml_rpc);
    }
    else
    {
        ROS_WARN("Parameter A0 not set, keeping default values...");
    }

    // Parameter A0
    XmlRpc::XmlRpcValue C0_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/C0"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/C0", C0_xml_rpc);
    }
    else
    {
        ROS_WARN("Parameter C0 not set, keeping default values...");
    }

    // Parameter A0
    XmlRpc::XmlRpcValue Damp_xml_rpc;
    if (nodehandle_.hasParam("/schunk_low_control/schunk/Damp"))
    {
        nodehandle_.getParam("/schunk_low_control/schunk/Damp", Damp_xml_rpc);
    }
    else
    {
        ROS_WARN("Parameter Damp not set, keeping default values...");
    }

    std::vector<int> A0;
    std::vector<int> C0;
    std::vector<int> Damp;

    A0.resize(A0_xml_rpc.size());
    C0.resize(C0_xml_rpc.size());
    Damp.resize(Damp_xml_rpc.size());

    for (int i = 0; i < A0_xml_rpc.size(); i++) A0[i] = static_cast<int>(A0_xml_rpc[i]);
    for (int i = 0; i < C0_xml_rpc.size(); i++) C0[i] = static_cast<int>(C0_xml_rpc[i]);
    for (int i = 0; i < Damp_xml_rpc.size(); i++) Damp[i] = static_cast<int>(Damp_xml_rpc[i]);

    return schunk_->SetPIDParameters(A0, C0, Damp);
}

/******************************************************************************
                                         PUBLISH FUNCTIONS
 *****************************************************************************/

void SchunkLowControl::PublishState()
{
    schunk_msgs::JointState simpl_msg;
    schunk_msgs::JointStateComplete compl_msg;
    schunk_msgs::JointStateElectrical elec_msg;

    compl_msg = schunk_->ReadJointStates();
    elec_msg = schunk_->ReadElectricalStates();
    simpl_msg.ModuleState = compl_msg.ModuleState;
    simpl_msg.Pos = compl_msg.Pos;
    simpl_msg.Vel = compl_msg.Vel;

    simpl_msg.header.stamp = ros::Time::now();
    compl_msg.header.stamp = simpl_msg.header.stamp;
    elec_msg.header.stamp = simpl_msg.header.stamp;

    publisher_simple_.publish(simpl_msg);
    publisher_complete_.publish(compl_msg);
    publisher_electrical_.publish(elec_msg);

    publisher_queue_.callAvailable((ros::WallDuration()));

    last_publish_time_ = ros::Time::now();
}

/******************************************************************************
                                          SUBSCRIBED TOPICS
 *****************************************************************************/

void SchunkLowControl::VelocityCallback(const schunk_msgs::JointVelocities::ConstPtr& msg)
{
    if (connected_ && msg->NID == connection_NID_)
    {
        std::vector<double> next_vel;
        if (has_gripper_)
            next_vel.resize(schunk_->module_count() - 1);
        else
            next_vel.resize(schunk_->module_count());

        CheckJointLimits(schunk_->GetCurrentPositions(), msg->velocities, &next_vel);
        schunk_->SetVelocitiesForNextPosture(next_vel);
        schunk_->Move();
    }
    else
    {
        ROS_WARN_STREAM("SchunkLowControl::velocityCallback - could not verify connection NID");
        ROS_WARN_STREAM("SchunkLowControl::velocityCallback - Received NID : " << msg->NID << ", Expected NID : " << connection_NID_);
    }
}

void SchunkLowControl::PositionCallback(const schunk_msgs::JointPositions::ConstPtr& msg)
{
    if (connected_ && msg->NID == connection_NID_)
    {
        std::vector<double> msg_pos = msg->positions;
        std::vector<double> cur_pos = schunk_->GetCurrentPositions();

        std::vector<double> next_pos;
        std::vector<double> next_vel;
        std::vector<double> next_acc;
        if (has_gripper_)
        {
            next_pos.resize(schunk_->module_count() - 1);
            next_vel.resize(schunk_->module_count() - 1);
            next_acc.resize(schunk_->module_count() - 1);
        }
        else
        {
            next_pos.resize(schunk_->module_count());
            next_vel.resize(schunk_->module_count());
            next_acc.resize(schunk_->module_count());
        }

        for (int i = 0; i < static_cast<int>(next_pos.size()); i++)
        {
            if (msg_pos[i] > cur_pos[i])
            {
                if (msg_pos[i] > max_pos_[i])
                    next_pos[i] = max_pos_[i];
                else
                    next_pos[i] = msg_pos[i];

                next_vel[i] = std_vel_[i];
            }
            else
            {
                if (msg_pos[i] < min_pos_[i])
                    next_pos[i] = min_pos_[i];
                else
                    next_pos[i] = msg_pos[i];

                next_vel[i] = -1.0 * std_vel_[i];
            }

            next_acc[i] = std_acc_[i];
        }

        schunk_->SetNextPosture(next_pos, next_vel, next_acc);
        schunk_->Move();
    }
    else
    {
        ROS_WARN_STREAM("SchunkLowControl::positionCallback - could not verify connection NID");
    }
}

/******************************************************************************
                                 SERVICES
 *****************************************************************************/

bool SchunkLowControl::InitCallback(schunk_low::Init::Request& req, schunk_low::Init::Response& resp)
{
    // If node is already initialized, there is nothing left to do
    if (initialized_)
    {
        ROS_INFO_STREAM("SchunkLowControl::InitCallback - Node already initialized");
        return true;
    }
    else
    {
        ROS_INFO_STREAM("SchunkLowControl::InitCallback - Initializing SchunkLowControl");

        // Initialize robot
        if (schunk_->Init())
        {
            if (has_gripper_)
            {
                if (!schunk_->InitGripper())
                {
                    ROS_ERROR_STREAM("SchunkLowControl::InitCallback - Could not initialize the gripper ");
                    return false;
                }
            }

            initialized_ = true;
            ROS_INFO_STREAM("SchunkLowControl::InitCallback - Node initialized successfully");

            schunk_msgs::AllParameters param_msg = schunk_->ReadAllParameters();
            param_msg.header.stamp = ros::Time::now();
            publisher_parameters_.publish(param_msg);
            publisher_queue_.callAvailable((ros::WallDuration()));

            return true;
        }

        else
        {
            ROS_ERROR_STREAM("SchunkLowControl::InitCallback - Could not initialize robot object");
            return false;
        }
    }

    return true;
}

bool SchunkLowControl::HomeCallback(schunk_low::Home::Request& req, schunk_low::Home::Response& resp)
{
    if (initialized_)
    {
        ROS_INFO_STREAM("SchunkLowControl::HomeCallback - Reseting all modules");
        schunk_->ResetAllModules();

        ROS_INFO_STREAM("SchunkLowControl::HomeCallback - Moving joints to the home position");
        schunk_->DoHardHoming(schunk_->module_count()-1);

        return true;
    }
    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::homeCallback - Node not initialized");
        return false;
    }
}

bool SchunkLowControl::StopCallback(schunk_low::Stop::Request& req, schunk_low::Stop::Response& resp)
{
    ROS_INFO_STREAM("SchunkLowControl::StopCallback - Stopping all modules ");

    // Stop robot
    if (schunk_->Stop())
    {
        ROS_INFO_STREAM("SchunkLowControl::StopCallback - Robot stopped successfully");
        sleep(1);
        return true;
    }
    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::StopCallback - Could not stop robot");
        return false;
    }
}

bool SchunkLowControl::RecoverCallback(schunk_low::Recover::Request& req, schunk_low::Recover::Response& resp)
{
    ROS_INFO_STREAM("SchunkLowControl::RecoverCallback - Recovering all modules ");

    if (initialized_)
    {
        // stop robot, if its still moving
        if (!schunk_->Stop())
        {
            ROS_WARN_STREAM("SchunkLowControl::RecoverCallback - Could not stop robot. callback function proceeding anyway");
        }
        sleep(1);

        // reset all modules
        if (!schunk_->ResetAllModules())
        {
            ROS_ERROR_STREAM("SchunkLowControl::RecoverCallback - Could not reset modules. recovery failed");
            return false;
        }
        sleep(1);

        return true;
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::RecoverCallback - Node not initialized. cannot recover modules");
        return false;
    }
}

bool SchunkLowControl::GripperOpenCallback(schunk_low::GripperOpen::Request& req, schunk_low::GripperOpen::Response& resp)
{
    if (initialized_ && has_gripper_)
    {
        // Get gripper index and ID
        int gripper_index = schunk_->module_count()-1;
        std::vector<int> module_IDs = schunk_->module_ids();
        int gripper_id = module_IDs[gripper_index];

        // Check if the gripper is not already opened
        std::vector<double> current_positions = schunk_->GetCurrentPositions();
        double gripper_position = current_positions[gripper_index];
        double target_position = req.pos * max_pos_[gripper_index];

        if (target_position > gripper_position)
        {
            schunk_->Reset(gripper_id);
            schunk_->SetJointValueForNextMove(gripper_id, target_position, std_vel_[gripper_index], std_acc_[gripper_index]);
            schunk_->Move();
            return true;
        }

        else
        {
            ROS_INFO_STREAM("SchunkLowControl::GripperOpenCallback - Gripper is already opened");
            return true;
        }
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::gripOpenCallback - Node not initialized");
        return false;
    }
}


bool SchunkLowControl::GripperCloseCallback(schunk_low::GripperClose::Request& req, schunk_low::GripperClose::Response& resp)
{
    if (initialized_ && has_gripper_)
    {
        // Get gripper index and ID
        int gripper_index = schunk_->module_count()-1;
        std::vector<int> module_IDs = schunk_->module_ids();
        int gripper_id = module_IDs[gripper_index];

        // Check if the gripper is not already opened
        std::vector<double> current_positions = schunk_->GetCurrentPositions();
        double gripper_position = current_positions[gripper_index];
        double target_position = req.pos * max_pos_[gripper_index];

        if (target_position < gripper_position)
        {
            schunk_->Reset(gripper_id);
            schunk_->SetJointValueForNextMove(gripper_id, target_position, std_vel_[gripper_index], std_acc_[gripper_index]);
            schunk_->Move();
            return true;
        }

        else
        {
            ROS_INFO_STREAM("SchunkLowControl::GripperCloseCallback - Gripper is already closed");
            return true;
        }
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::GripperCloseCallback - Node not initialized");
        return false;
    }
}

bool SchunkLowControl::GripperSetVelCallback(schunk_low::GripperSetVel::Request& req, schunk_low::GripperSetVel::Response& resp)
{
    if (initialized_ && has_gripper_)
    {
        int gripper_index = schunk_->module_count()-1;
        std::vector<int> module_IDs = schunk_->module_ids();
        int gripper_id = module_IDs[gripper_index];

        schunk_->Reset(gripper_id);
        schunk_->SetJointVelocityForNextMove(gripper_id, req.velocity);
        schunk_->Move();

        return true;
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::gripSetVelCallback - node not initialized");
        return false;
    }
}

bool SchunkLowControl::ConnectCallback(schunk_low::Connect::Request& req, schunk_low::Connect::Response& resp)
{
    if (initialized_)
    {
        if (!connected_)
        {
            connected_ = true;
            connection_NID_ = req.NID;
            return true;
        }

        else
        {
            ROS_ERROR_STREAM("SchunkLowControl::ConnectCallback - node already connected to another node");
            return false;
        }
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::ConnectCallback - node not initialized");
        return false;
    }
}

bool SchunkLowControl::DisconnectCallback(schunk_low::Disconnect::Request& req, schunk_low::Disconnect::Response& resp)
{
    if (initialized_)
    {
        if (connected_)
        {
            connected_ = false;
            return true;
        }

        else
        {
            ROS_ERROR_STREAM("SchunkLowControl::DisconnectCallback - node not connected to another node");
            return false;
        }
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::DisconnectCallback - node not initialized");
        return false;
    }
}


bool SchunkLowControl::SetControlModeCallback(schunk_low::ControlMode::Request& req, schunk_low::ControlMode::Response& resp)
{
    if (initialized_)
    {
        if (connected_)
        {
            unsigned int control_mode = req.mode;

            if (control_mode > MIN_CONTROL_MODE && control_mode < MAX_CONTROL_MODE)
            {
                control_mode_ = (ControlMode)control_mode;
                ROS_INFO_STREAM("SchunkLowControl::SetControlModeCallback - Control mode successfully changed to " << control_mode);
                return true;
            }
            else
            {
                ROS_ERROR_STREAM("SchunkLowControl::SetControlModeCallback - Invalid control mode: " << control_mode << " (value must be between " << MIN_CONTROL_MODE + 1 << " and " << MAX_CONTROL_MODE - 1 << ")");
                return false;
            }
        }

        else
        {
            ROS_ERROR_STREAM("SchunkLowControl::SetControlModeCallback - Node not connected to another node");
            return false;
        }
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::SetControlModeCallback - Node not initialized");
        return false;
    }
}



bool SchunkLowControl::GetPosVelCallback(schunk_low::GetPosVel::Request& req, schunk_low::GetPosVel::Response& resp)
{
    if (initialized_)
    {
        resp.positions = schunk_->GetCurrentPositions();
        resp.velocities = schunk_->GetCurrentVelocities();

        return true;
    }

    else
    {
        ROS_ERROR_STREAM("SchunkLowControl::GetPosVelCallback - node not initialized");
        return false;
    }
}

/******************************************************************************
                             AUXILIAR FUNCTIONS
 *****************************************************************************/

bool SchunkLowControl::CheckJointLimits(const std::vector<double>& cur_pos, const std::vector<double>& next_vel, std::vector<double> *limited_vel)
{
    bool limited = false;
    for (int i = 0; i < static_cast<int>(((*limited_vel).size())); i++)
    {
        if (next_vel[i] > 0)
        {
            if (cur_pos[i] + 2 * (next_vel[i] / frequency_) >= max_pos_[i])
            {
                (*limited_vel)[i] = 0.0;
                limited = true;
            }
            else
            {
                (*limited_vel)[i] = next_vel[i];
            }
        }
        else if (next_vel[i] < 0)
        {
            if (cur_pos[i] + 2 * (next_vel[i] / frequency_) <= min_pos_[i])
            {
                (*limited_vel)[i] = 0.0;
                limited = true;
            }
            else
            {
                (*limited_vel)[i] = next_vel[i];
            }
        }
    }

    return limited;
}
